/*
 * @Description: 点云预处理模块，包括时间同步，点云去畸变等，这里相当于未做操作
 * @Author: Sang Hao
 * @Date: 2021-10-26 11:49:39
 * @LastEditTime: 2021-11-22 22:04:47
 * @LastEditors: Sang Hao
 */

#include <yaml-cpp/yaml.h>
#include "glog/logging.h"

#include "lidar_slam/data_pretreat/data_pretreat_flow.hpp"
#include "lidar_slam/global_defination/global_defination.h"
#include "lidar_slam/models/cloud_filter/outlier_filter.hpp"
#include "lidar_slam/models/cloud_filter/no_filter.hpp"
#include "lidar_slam/models/cloud_filter/range_filter.hpp"
#include "lidar_slam/models/cloud_filter/multi_plane_clipper_filter.hpp"
#include "lidar_slam/models/extract/plane_ransac_extract.hpp"

namespace lidar_slam {
DataPretreatFlow::DataPretreatFlow(ros::NodeHandle& nh) {

    InitWithConfig();
    // subscriber
    cloud_sub_ptr_ = std::make_shared<CloudSubscriber>(nh, topic_name_, 100000);
    // velocity_sub_ptr_ = std::make_shared<VelocitySubscriber>(nh, "/kitti/oxts/gps/vel", 1000000);
    lidar_to_imu_ptr_ = std::make_shared<TFListener>(nh, "/imu_link", "velo_link");
    // publisher
    cloud_pub_ptr_ = std::make_shared<CloudPublisher>(nh, "/synced_cloud", "/velo_link", 100);

    distortion_adjust_ptr_ = std::make_shared<DistortionAdjust>();

}

bool DataPretreatFlow::InitWithConfig() {
    std::string config_file_path = WORK_SPACE_PATH + "/config/data_pretreat.yaml";
    YAML::Node config_node = YAML::LoadFile(config_file_path);

    std::cout << "----------------数据预处理初始化--------------" << std::endl;
    topic_name_ = config_node["pointcloud2_topic_name"].as<std::string>();

    std::cout << "订阅点云话题: " << topic_name_ << std::endl;

    InitFilter(config_node);

    return true;
}

/**
 * @description: 其实这里的滤波完全可以直接定义为outlier_filter，不用考虑其他方法，直接给参数即可，也不用给User
 * @param  {*}
 * @return {*}
 */
bool DataPretreatFlow::InitFilter(const YAML::Node& config_node) {
    /* 初始化距离滤波 */
    if (config_node["use_range_filter"].as<bool>()) {
        /* 不能对私有变量进行更改吗？能访问吗？ */
        use_range_filter_ = true;
        range_filter_ptr_ = std::make_shared<RangeFilter>(config_node["range_filter"]);
        std::cout << "完成距离滤波的初始化!" << std::endl;
    }    

    /* 初始化outlier_filter滤波方法 */
    if (config_node["use_outlier_filter"].as<bool>()) {
        use_outlier_filter_ = true;
        outlier_filter_ptr_ = std::make_shared<OutlierFilter>(config_node["outlier_filter"]);
        std::cout << "完成统计滤波的初始化!" << std::endl;
    }

    /* 初始化平面裁剪器 */
    if (config_node["use_multi_plane_clipper_filter"].as<bool>()) {
        use_multi_plane_clipper_filter_ = true;
        multi_plane_clipper_filter_ptr_ = std::make_shared<MultiPlaneClipperFilter>(
            config_node["multi_plane_clipper_filter"]);
        std::cout << "完成多平面裁剪器的初始化!" << std::endl;
    }

    return true;
}


bool DataPretreatFlow::Run() {
    if (!ReadData())
        return false;

    if (!InitCalibration()) 
        return false;

    while(HasData()) {
        if (!ValidData())
            continue;
        FilterData();
        TransformData();
        PublishData();
    }

    return true;
}

bool DataPretreatFlow::ReadData() {
    cloud_sub_ptr_->ParseData(cloud_data_buff_);

    /* 姑且也算是私有变量 */
    // static std::deque<VelocityData> unsynced_velocity_;
    /* 将订阅到的速度信息放入到unsynced_velocity_中 */
    // velocity_sub_ptr_->ParseData(unsynced_velocity_);

    if (cloud_data_buff_.size() == 0)
        return false;

    double cloud_time = cloud_data_buff_.front().time;
    static VelocityData velocity_;
    velocity_.time = cloud_time;
    bool valid_velocity = true; /* 直接认为同步了 */
    // bool valid_velocity = VelocityData::SyncData(unsynced_velocity_, velocity_data_buff_, cloud_time);

    /* 在直接自定义同步的状态下，这一段代码其实无意义了 */
    static bool sensor_inited = false;
    if (!sensor_inited) {
        if (!valid_velocity) {
            cloud_data_buff_.pop_front();
            return false;
        }
        sensor_inited = true;
    }

    return true;
}

bool DataPretreatFlow::InitCalibration() {
    static bool calibration_received = false;
    if (!calibration_received) {
        // if (lidar_to_imu_ptr_->LookupData(lidar_to_imu_)) {
            calibration_received = true;
        // }
    }

    return calibration_received;
}



bool DataPretreatFlow::HasData() {
    if (cloud_data_buff_.size() == 0)
        return false;
    // if (velocity_data_buff_.size() == 0)
        // return false;

    return true;
}

/* 数据有效性处理上，用pcl_isfinite来去除一些无效点 */
/* 更进一步，判断数据是否有效应看多传感器的时间同步，和检测是否丢帧，但是这里暂不考虑同步和丢帧 */
bool DataPretreatFlow::ValidData() {
    current_cloud_data_ = cloud_data_buff_.front();
    auto it = current_cloud_data_.cloud_ptr->points.begin();
    while (it != current_cloud_data_.cloud_ptr->points.end()) {
		if (!pcl_isfinite(it->x) || !pcl_isfinite(it->y) || !pcl_isfinite(it->z)) {
			/* 去除无效点，用pcl::removeNANFromPointCloud()函数会出现一些问题 */
            it = current_cloud_data_.cloud_ptr->points.erase(it);
		} else {
			++it;
        }
	}
    cloud_data_buff_.pop_front();

    return true;
}

/* TODO 似乎不太对，这里的输入点云是const限定的，这怎么能修改呢？
    但是之前只有voxel滤波和outlier_filter是都可以 */
bool DataPretreatFlow::FilterData() {
    if (use_range_filter_) {
        range_filter_ptr_->Filter(current_cloud_data_.cloud_ptr, 
            current_cloud_data_.cloud_ptr);
    }

    if (use_outlier_filter_) {
        outlier_filter_ptr_->Filter(current_cloud_data_.cloud_ptr, 
            current_cloud_data_.cloud_ptr);
    }
    
    if (use_multi_plane_clipper_filter_) {
        /* 这里即便结果是false也无妨了，因为无非就是不滤波而已 */
        multi_plane_clipper_filter_ptr_->Filter(current_cloud_data_.cloud_ptr, 
            current_cloud_data_.cloud_ptr);
    }

    return true;
}

bool DataPretreatFlow::TransformData() {

    current_velocity_data_.TransformCoordinate(lidar_to_imu_);
    /* param:扫描周期 */
    distortion_adjust_ptr_->SetMotionInfo(0.1, current_velocity_data_);
    distortion_adjust_ptr_->AdjustCloud(current_cloud_data_.cloud_ptr, current_cloud_data_.cloud_ptr);

    return true;
}

bool DataPretreatFlow::PublishData() {
    cloud_pub_ptr_->Publish(current_cloud_data_.cloud_ptr, current_cloud_data_.time);

    return true;
}
}